﻿/*
 * ©2009-2010 XELF
 * http://xelf.info
 */

using System;
#if XNA || SILVERLIGHT || WINDOWS_PHONE
using Microsoft.Xna.Framework;
using _MathHelper = Microsoft.Xna.Framework.MathHelper;
#else
using _MathHelper = XELF.Framework.MathHelper;
#endif

namespace XELF.Framework {
#if XNA || SILVERLIGHT || WINDOWS_PHONE
	public static partial class QuaternionHelper {
#else
	public partial struct Quaternion {
#endif

		/// <summary>
		/// 回転前の方向ベクトルと回転後の方向ベクトルから回転クォータニオンを作る。
		/// </summary>
		/// <param name="from">回転前の方向ベクトル</param>
		/// <param name="to">回転後の方向ベクトル</param>
		/// <returns></returns>
		public static Quaternion CreateFromDirectionToDirection(Vector3 from, Vector3 to) {
			float d;
			Vector3.Dot(ref from, ref to, out d);
			var qw = MathHelper.Sqrt(from.LengthSquared() * to.LengthSquared()) + d;
			Quaternion result;
			if (qw < 0.0001f) {
				result = new Quaternion(-from.Z, from.Y, from.X, 0);
			} else {
				Vector3 axis;
				Vector3.Cross(ref from, ref to, out axis);
				result = new Quaternion(axis.X, axis.Y, axis.Z, qw);
			}
			result.Normalize();
			return result;
		}
#if DEBUG
		/// <summary>
		/// 回転前の方向ベクトルと回転後の方向ベクトルから回転クォータニオンを作る。
		/// </summary>
		/// <param name="from">回転前の方向ベクトル</param>
		/// <param name="to">回転後の方向ベクトル</param>
		/// <returns></returns>
		public static Quaternion CreateFromDirectionToDirectionBasic(Vector3 from, Vector3 to) {
			Vector3 axis;
			float angle;
			Vector3.Cross(ref from, ref to, out axis);
			if (axis.LengthSquared() < 1e-6f)
				return new Quaternion(-from.Z, from.Y, from.X, 0);
			axis.Normalize();
			Vector3.Dot(ref from, ref to, out angle);
			angle = _MathHelper.Clamp(angle, -1, 1);
			angle = (float)Math.Acos(angle);
			if (angle < 1e-6f)
				return new Quaternion(axis, 0);
			Quaternion result;
			Quaternion.CreateFromAxisAngle(ref axis, angle, out result);
			return result;
		}
#endif
	}
}

